"""
需求：动作服务端，需要解析客户端提交的数字，遍历该数字并累加求和，最终结果响应回
     客户端，且请求响应过程中需要生存连续反馈。
流程：
    1.导包
    2.初始化ROS2客户端
    3.自定义节点类：
      3-1.创建动作服务端对象
      3-2.处理提交的目标值(操作对应一个专门的回调函数) --默认实现
      3-3.处理取消请求(操作对应一个专门的回调函数) --默认实现
      3-4.生成连续反馈 与 最终响应(操作对应一个专门的回调函数)
    4.调用spin函数，传入自定义类对象指针，
    spin又称回旋函数，当执行到spin时，又会回去执行一次函数，而不是退出，可以理解为for语句
    5.释放资源
"""
 
# 1.导包
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer
from base_interfaces_demo.action import Progress
import time
 
# 3.自定义节点类: 
class ProgressActionServer(Node):
    def __init__(self):
        super().__init__("progress_action_server_node_py")
        self.get_logger().info("动作通信服务端创建(Python)!")
        # 3-1.创建动作服务端对象
        # node: Any, 创建时依赖的一个节点
        # action_type: Any, 自定义接口类型
        # action_name: Any, 话题名称
        # execute_callback， 主逻辑，生成连续反馈 与 最终响应
        self.server = ActionServer(
            self,
            Progress,
            "get_sum",
            self.excute_callback
        )

    # 3-4.生成连续反馈 与 最终响应(操作对应一个专门的回调函数)
    def excute_callback(self,goal_handle):
        # 1.生成连续反馈
        # 首先，获取目标值，然后遍历，遍历中累加，且每循环一次就计算进度，并作为连续反馈
        num = goal_handle.request.num
        sum = 0
        for i in range(1, num + 1):
            sum += i
            feedback  = Progress.Feedback()
            feedback.progress = i / num 
            goal_handle.publish_feedback(feedback)
            self.get_logger().info("连续反馈:%.2f" % feedback.progress)
            time.sleep(1.0)    # 每次循环，休眠1秒

        # 2.响应最终结果
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum 

        self.get_logger().info("计算结果:%d" % result.sum)
        return result
    
 
def main():
    # 2.初始化ROS2客户端
    rclpy.init()
 
    # 4.调用spin函数，传入自定义类对象指针，
    rclpy.spin(ProgressActionServer())
 
    # 5.释放资源
    rclpy.shutdown()
 
if __name__ == '__main__':
    main()